#ifndef VISION_TASK_H
#define VISION_TASK_H

#include "robot_config.h"
#include "usb_typdef.h"
#include  "user_lib.h"
#define VISION_TIME 5 //ms

#define VISION_CONTROL_TIME 0.002f

#define VISION_PITCH_NUM 0.1666666667f
#define VISION_YAW_NUM 0.3333333333f

// 角度滤波（带范围检查）
#define YAW_MIN -PI
#define YAW_MAX PI

// 完整定义 SolveAns 结构体
typedef struct 
{
    bool tracking;      //是否跟踪
    float pitch_v;        //自己pitch
    float yaw_v;          //自己yaw
    float aim_x;        //目标x
    float aim_y;        //目标y
    float aim_z;        //目标z
} SolveAns;


// 数据滤波器集合
typedef struct {
    KalmanFilter xw_filter;     // x坐标滤波
    KalmanFilter yw_filter;     // y坐标滤波
    KalmanFilter zw_filter;     // z坐标滤波
    KalmanFilter vxw_filter;    // x速度滤波
    KalmanFilter vyw_filter;    // y速度滤波
    KalmanFilter vzw_filter;    // z速度滤波
    KalmanFilter tar_yaw_filter;// 目标yaw滤波
} VisionDataFilters;

// 声明外部函数
extern void vision_task(void const * argument);
extern SolveAns* GetSolveAns(void);
extern bool get_tracking_status(void);
#endif
